#include "can.hpp"
#include <iostream>
using namespace CANalyst2;

VCI_BOARD_INFO pInfo; // 用来获取设备信息。
int count = 0;        // 数据列表中，用来存储列表序号。
VCI_BOARD_INFO pInfo1[50];
int num = 0;

UsbCAN::UsbCAN()
{
    num = VCI_FindUsbDevice2(pInfo1);
    printf(">>USBCAN DEVICE NUM:");
    printf("%d", num);
    printf(" PCS");
    printf("\n");

    if (VCI_OpenDevice(VCI_USBCAN2, 0, 0) == 1) // 打开设备
    {
        is_can_usb_opened = true;
        printf(">>open deivce success!\n"); // 打开设备成功
    }
    else
    {
        is_can_usb_opened = false;
        printf(">>open deivce error!\n");
    }
    

    if (is_can_usb_opened && (VCI_ReadBoardInfo(VCI_USBCAN2, 0, &pInfo) == 1)) // 读取设备序列号、版本等信息。
    {
        // for (int i = 0; i < 20; i++)
        // {
        //     printf(">>Serial_Num:");
        //     printf("%c ", pInfo.str_Serial_Num[i]);
        //     printf("\n");
        // }
        // for (int i = 0; i < 10; i++)
        // {
        //     printf(">>hw_Type:");
        //     printf("%c ", pInfo.str_hw_Type[i]);
        //     printf("\n");
        // }
        // printf(">>Firmware Version:V");
        // printf("%x", (pInfo.fw_Version & 0xF00) >> 8);
        // printf(".");
        // printf("%x", (pInfo.fw_Version & 0xF0) >> 4);
        // printf("%x", pInfo.fw_Version & 0xF);
        // printf("\n");
    }
    else
    {
        printf(">>Get VCI_ReadBoardInfo error!\n");
    }
};

UsbCAN::~UsbCAN()
{
    VCI_ResetCAN(VCI_USBCAN2, 0, 0); // 复位CAN1通道。
    usleep(100000);                  // 延时100ms。
    VCI_ResetCAN(VCI_USBCAN2, 0, 1); // 复位CAN2通道。
    usleep(100000);                  // 延时100ms。
    if (is_can_usb_opened) {
        VCI_CloseDevice(VCI_USBCAN2, 0); // 关闭设备。
    }   
}

int UsbCAN::open(CanBaud baud, int index)
{

    VCI_INIT_CONFIG config;
    config.AccCode = 0;
    config.AccMask = 0xFFFFFFFF;
    config.Filter = 1;     // 接收所有帧
    // config.Timing0 = 0x00; /*波特率500 Kbps*/
    // config.Timing1 = 0x1C;
    config.Mode = 0; // 正常模式

    switch (baud) {
        case CanBaud::BAUD_1M:
            config.Timing0 = 0x00;
            config.Timing1 = 0x14;
            break;
        case CanBaud::BAUD_500K:
            config.Timing0 = 0x00;
            config.Timing1 = 0x1C;
            break;
        case CanBaud::BAUD_250K:
            config.Timing0 = 0x01;
            config.Timing1 = 0x1C;
            break;
        case CanBaud::BAUD_125K:
            config.Timing0 = 0x03;
            config.Timing1 = 0x1C;
            break;
        default:
            printf("Unsupported baudrate\n");
            return -1;
    }

    if (VCI_InitCAN(VCI_USBCAN2, 0, index, &config) != 1)
    {
        printf(">>Init CAN%d error\n", index);
        VCI_CloseDevice(VCI_USBCAN2, index);
        return -1;
    }
    else
    {
        printf(">>Init CAN%d success\n", index);
    }

    if (VCI_StartCAN(VCI_USBCAN2, 0, index) != 1)
    {
        printf(">>Start CAN%d error\n", index);
        VCI_CloseDevice(VCI_USBCAN2, index);
        return -1;
    }
    else
    {
        printf(">>Start CAN%d success\n", index);
    }
    return 0;
}

int UsbCAN::send(std::vector<CanStamped> &buffer, int timeout_msec, int index)
{
    // for (int i = 0; i < buffer.size(); ++i)
    // {
    //     auto &frame = buffer[i];
    //     int reclen = VCI_Transmit(VCI_USBCAN2, 0, index, reinterpret_cast<PVCI_CAN_OBJ>(&frame), 1);
    //     if (reclen == 1)
    //     {
    //         printf("CAN1 TX ID:0x%08X", buffer[0].ID);
    //         if (buffer[0].ExternFlag == 0)
    //         {
    //             printf(" Standard ");
    //         }
    //         if (buffer[0].ExternFlag == 1)
    //         {
    //             printf(" Extend   ");
    //         }
    //         if (buffer[0].RemoteFlag == 0)
    //         {
    //             printf(" Data   ");
    //         }
    //         if (buffer[0].RemoteFlag == 1)
    //         {
    //             printf(" Remote ");
    //         }
    //         printf("DLC:0x%02X", buffer[0].DataLen);
    //         printf(" data:0x");

    //         for (int i = 0; i < buffer[0].DataLen; i++)
    //         {
    //             printf(" %02X", buffer[0].Data[i]);
    //         }
    //         printf("\n");
    //     }
    //     else if (reclen == -1)
    //     {
    //         printf("USBCAN device offline!!!");
    //         return -1;
    //     }
    //     else if (reclen == 0)
    //     {
    //         printf("USBCAN send failed!!!\n");
    //         return -1;
    //     }
    //     else
    //     {
    //         printf("USBCAN send error!!!\n");
    //         return -1;
    //     }
    // }
    // return 0;
    for (int i = 0; i < buffer.size(); ++i)
    {
        auto &frame = buffer[i];
        // print frame INFO
        std::cout << ">>> VCI_Transmit frame"
                  << " ID: 0x" << std::hex << frame.ID << std::dec
                  << " DLC: " << static_cast<int>(frame.DataLen)
                  << " Data: ";
        for (int j = 0; j < frame.DataLen; ++j)
        {
            printf("%02X ", frame.Data[j] & 0xFF);
        }
        std::cout << std::endl;

        int reclen = VCI_Transmit(VCI_USBCAN2, 0, index, reinterpret_cast<PVCI_CAN_OBJ>(&frame), 1);
    }
    return 0;
}

int UsbCAN::recv(std::vector<CanStamped> &buffer, int timeout_msec, int index)
{
    // buffer.resize(2048);
    // for (int i = 0; i < buffer.size(); ++i)
    // {
    //     auto &frame = buffer[i];
    //     int reclen = VCI_Receive(VCI_USBCAN2, 0, index, reinterpret_cast<PVCI_CAN_OBJ>(&frame), 2048, 100);
    //     if (reclen > 0)
    //     {
    //         printf("recv res: %d\n", reclen);
    //         printf("CAN RX ID:0x%08X", buffer[0].ID); // ID
    //         if (buffer[0].ExternFlag == 0)
    //             printf(" Standard "); // 帧格式：标准帧
    //         if (buffer[0].ExternFlag == 1)
    //             printf(" Extend   "); // 帧格式：扩展帧
    //         if (buffer[0].RemoteFlag == 0)
    //             printf(" Data   "); // 帧类型：数据帧
    //         if (buffer[0].RemoteFlag == 1)
    //             printf(" Remote ");                  // 帧类型：远程帧
    //         printf("DLC:0x%02X", buffer[0].DataLen); // 帧长度
    //         printf(" data:0x");                      // 数据
    //         for (int i = 0; i < buffer[0].DataLen; i++)
    //         {
    //             printf(" %02X", buffer[0].Data[i]);
    //         }
    //         printf(" TimeStamp:0x%08X", buffer[0].TimeStamp); // 时间标识。
    //         printf("\n");
    //     }
    //     else if (reclen == -1)
    //     {
    //         printf("USBCAN device offline!!!");
    //         return -1;
    //     }
    //     else if (reclen == 0)
    //     {
    //         if (i == 0)
    //         {
    //             printf("USBCAN receive no data!!!\n");
    //             return -1;
    //         }
    //         else
    //         {
    //             return 0;
    //         }
    //     }
    //     else
    //     {
    //         printf("USBCAN receive error!!!\n");
    //         return -1;
    //     }
    // }
    // return 0;

    buffer.resize(1024); // 能容纳的最大帧数
    int reclen = VCI_Receive(
        VCI_USBCAN2, 
        0, 
        index, 
        reinterpret_cast<PVCI_CAN_OBJ>(buffer.data()), 
        buffer.size(), 
        timeout_msec
    );
    buffer.resize(reclen); // 实际收到的帧数

    if (reclen > 0) {    
        for (int i = 0; i < reclen; ++i) {
            auto &frame = buffer[i];
            printf("<<< VCI_Receive frame  ID: 0x%03X DLC: %d Data:", frame.ID, frame.DataLen);
            for (int j = 0; j < frame.DataLen; ++j)
                printf(" %02X", frame.Data[j]);
            printf("\n");
        }
        return 0;
    }
    else if (reclen == 0) {
        printf("No CAN frame received\n");
        return -1;
    }
    else {
        printf("CAN receive error!\n");
        return -1;
    }
}

int UsbCAN::send(std::vector<CanStamped> &buffer, int timeout_msec)
{
    for (int i = 0; i < buffer.size(); ++i)
    {
        auto &frame = buffer[i];
        // print frame INFO
        std::cout << ">>> VCI_Transmit frame"
                  << " ID: 0x" << std::hex << frame.ID << std::dec
                  << " DLC: " << static_cast<int>(frame.DataLen)
                  << " Data: ";
        for (int j = 0; j < frame.DataLen; ++j)
        {
            printf("%02X ", frame.Data[j] & 0xFF);
        }
        std::cout << std::endl;

        int reclen = VCI_Transmit(VCI_USBCAN2, 0, 1, reinterpret_cast<PVCI_CAN_OBJ>(&frame), 1);
        // if (reclen == 1)
        // {
        //     continue;
        // }
        // else if (reclen == -1)
        // {
        //     printf("USBCAN device offline!!!");
        //     return -1;
        // }
        // else if (reclen == 0)
        // {
        //     printf("USBCAN send failed!!!\n");
        //     return -1;
        // }
        // else
        // {
        //     printf("USBCAN send error!!!\n");
        //     return -1;
        // }
    }
    return 0;
}

// int UsbCAN::recv(std::vector<CanStamped> &buffer, int timeout_msec)
// {
//     std::cout << "UsbCAN::recv" << std::endl;
//     buffer.resize(1024);
//     for (int i = 0; i < buffer.size(); ++i)
//     {

//         auto &frame = buffer[i];
//         // CanStamped frame_test;
//         // std::cout << "VCI_Receive" << std::endl;
//         int reclen = VCI_Receive(VCI_USBCAN2, 0, 0, reinterpret_cast<PVCI_CAN_OBJ>(&frame), 3000, 1000);
//         if (reclen > 0)
//         {
//             return 0;
//         }
//         else if (reclen == -1)
//         {
//             printf("USBCAN device offline!!!");
//             return -1;
//         }
//         else if (reclen == 0)
//         {
//             if (i == 0)
//             {
//                 printf("USBCAN receive no data!!!\n");
//                 return -1;
//             }
//             else
//             {
//                 std::cout << "VCI_Receive frame:" 
//                         << " ID: 0x" << std::hex << frame.ID
//                         << " DLC:" << std::dec << (int)frame.DataLen
//                         << " Data:";
//                 for(int j = 0; j < frame.DataLen; j++) {
//                     std::cout << std::hex << std::uppercase << (int)frame.Data[j] << " ";
//                 }
//                 std::cout << std::endl;
//                 return 0;
//             }
//         }
//         else
//         {
//             printf("USBCAN receive error!!!\n");
//             return -1;
//         }
//     }
//     return 0;
// }


int UsbCAN::recv(std::vector<CanStamped> &buffer, int timeout_msec)
{
    buffer.resize(1024); // 能容纳的最大帧数
    int reclen = VCI_Receive(
        VCI_USBCAN2, 
        0, 
        1, 
        reinterpret_cast<PVCI_CAN_OBJ>(buffer.data()), 
        buffer.size(), 
        timeout_msec
    );
    buffer.resize(reclen); // 实际收到的帧数

    if (reclen > 0) {    
        for (int i = 0; i < reclen; ++i) {
            auto &frame = buffer[i];
            printf("<<< VCI_Receive frame  ID: 0x%03X DLC: %d Data:", frame.ID, frame.DataLen);
            for (int j = 0; j < frame.DataLen; ++j)
                printf(" %02X", frame.Data[j]);
            printf("\n");
        }
        return 0;
    }
    else if (reclen == 0) {
        printf("No CAN frame received\n");
        return -1;
    }
    else {
        printf("CAN receive error!\n");
        return -1;
    }
}
